import serial
import math
import struct
import os
import time

class Ser:
    '''
    串口通信的工具类，用于与STM32进行数据交互
    通过串口发送数据到STM32
    参数：
        port: 串口设备路径，默认为'/dev/ttyS1'
        baudrate: 波特率，默认为115200
    内置函数：
        send_to_stm32: 发送数据到STM32并接收响应    
    用法：
        ser = Ser()
        angles = ...  # 六个关节的角度
        ser.send_to_stm32(angles)
    '''
    def __init__(self, baudrate=115200, port='/dev/ttyS1'):
        print("List of enabled UART:")
        os.system('ls /dev/tty[a-zA-Z]*')
        self.ser = serial.Serial(port, baudrate, timeout=1)
        if self.ser.isOpen():
            print(self.ser.name, 'Open Successed!')
        else:
            print('Open Failed!')

    def send_to_stm32(self, angles: list) -> None:
        '''
        将数据发送给stm32
        '''
        def get_crc8(data: bytes) -> bytes:
            """
            计算CRC8校验值，多项式0x8C
            :param data: 需要校验的字节数据
            :return: CRC8校验值（0~255）
            """
            crc = 0
            for byte in data:
                crc ^= byte
                for _ in range(8):
                    if crc & 0x01:
                        crc = (crc >> 1) ^ 0x8C
                    else:
                        crc >>= 1
                    crc &= 0xFF  # 保证crc始终为8位
            crc = bytes([crc])  # 转换为字节类型
            return crc

        if len(angles) != 6:
            raise ValueError("角度列表必须仅含6个元素")
        
        # 数据打包部分
        begin = b'\x55\xaa'
        end = b'\x0d\x0a'  # 结束符
        length = bytes([13])
        angles = [int(math.pi / 4 * 1000)] * 6  # 六个关节的角度
        print(angles)
        flag = b'\x00'   # 标志位
        # 六个关节的角度(弧度制) *1000, short类型

        # 将角度转换为short类型的字节数据
        angles_data = struct.pack('<hhhhhh', *angles)
        # 计算CRC8校验值
        crc = get_crc8(begin + length + angles_data + flag)
        # 最终发送的数据(bytes类型)
        out_data = begin + length + angles_data + flag + crc + end
        print(out_data) 

        # 串口发送部分
        ser = self.ser

        len_out_data = ser.write(out_data)
        print('输出字节长度:', len_out_data)


        # # 下面是接收数据
        # input_data = ser.read(19)
        # print('input_data:', input_data)

        # begin_in = input_data[0:2]
        # if begin_in == b'\x55\xaa':
        #     print('begin_in: CORRECT')

        # angles_in = input_data[3:15]
        # angles_in = struct.unpack('<hhhhhh', angles_in)
        # print('angles_in:', angles_in)

        # end_in = input_data[17:]
        # if end_in == b'\x0d\x0a':
        #     print('end_in: CORRECT')

        while 1:
            time.sleep(0.1)
            ser.write(out_data)

        ser.close()